/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/common/time/time.h"
#include <chrono>

#include "modules/planning/integration_tests/planning_test_base.h"

#include "cyber/common/file.h"
#include "cyber/common/log.h"

#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
#include "modules/routing/proto/routing.pb.h"

#include "include/global_defination.h" // 定义了 WORK_SPACE_PATH

namespace apollo
{
    namespace planning
    {
        // 绝对路径
        // /home/test_algorithm/algorithm/apollo_cmake_raw_version/src/apollo
        std::string pre_path_base = WORK_SPACE_PATH;

        using apollo::canbus::Chassis;
        using apollo::common::time::Clock;
        using apollo::localization::LocalizationEstimate;
        using apollo::perception::TrafficLightDetection;
        using apollo::prediction::PredictionObstacles;
        using apollo::routing::RoutingResponse;

        // PlanningBase指针
        std::unique_ptr<PlanningBase> planning_ = nullptr;
        std::map<TrafficRuleConfig::RuleId, bool> rule_enabled_;
        // 规划出的轨迹点
        ADCTrajectory adc_trajectory_;
        // planning模块的输入: localization,routing,obstacle,chassis
        LocalView local_view_;
        // planning模块的配置参数
        PlanningConfig config_;

        // 文件路径地址的前缀
        DEFINE_string(pre_dir, "", "the test data folder");
        // 是否写进文件
        DEFINE_bool(write_into_file, false, "true to update decision golden log file.");
        // routing_msg文件的地址
        DEFINE_string(test_routing_response_file, "", "The routing file used in test");
        // localization_msg文件的地址
        DEFINE_string(test_localization_file, "", "The localization test file");
        // chassis_msg文件的地址
        DEFINE_string(test_chassis_file, "", "The chassis test file");
        // prediction_msg文件的地址
        DEFINE_string(test_prediction_file, "", "The prediction module test file");
        // traffic_light_msg文件的地址
        DEFINE_string(test_traffic_light_file, "", "The traffic light test file");
        // relative_map_msg文件的地址
        DEFINE_string(test_relative_map_file, "", "The relative map test file");
        // 上一帧planning规划出的路径点的文件的地址
        DEFINE_string(test_previous_planning_file, "",
                      "The previous planning test file");

        void Set_file_names()
        {
            FLAGS_use_multi_thread_to_add_obstacles = false;
            FLAGS_enable_multi_thread_in_dp_st_graph = false;
            // planning模块的配置文件
            FLAGS_planning_config_file = pre_path_base +
                                         "/modules/planning/conf/planning_config.pb.txt";
            // traffic_rule的配置文件
            FLAGS_traffic_rule_config_filename = pre_path_base +
                                                 "/modules/planning/conf/traffic_rule_config.pb.txt";
            // 平滑器的配合文件
            FLAGS_smoother_config_filename = pre_path_base +
                                             "/modules/planning/conf/qp_spline_smoother_config.pb.txt";

            // planning模块相关的参数设置
            FLAGS_align_prediction_time = false;
            FLAGS_estimate_current_vehicle_state = false;
            FLAGS_enable_reference_line_provider_thread = false;
            // FLAGS_enable_trajectory_check is temporarily disabled, otherwise EMPlanner
            // and LatticePlanner can't pass the unit test.
            FLAGS_enable_trajectory_check = false;
            FLAGS_planning_test_mode = true;
            FLAGS_enable_lag_prediction = false;
            FLAGS_use_osqp_optimizer_for_reference_line = false;
            FLAGS_use_navigation_mode = false;
            FLAGS_planning_upper_speed_limit = 12.5;
            FLAGS_enable_scenario_stop_sign = false;
            FLAGS_enable_scenario_traffic_light = false;
            FLAGS_enable_rss_info = false;
            // Do not use fallback trajectory during testing
            FLAGS_use_planning_fallback = false;

            // traffic_rule的设置
            rule_enabled_[TrafficRuleConfig::CROSSWALK] = false;

            // 全局变量,载入的地图  // apollo/modules/map/hdmap/hdmap_util.cc 载入地图
            FLAGS_map_dir = pre_path_base + "/modules/map/data/sunnyvale_loop";
            FLAGS_test_base_map_filename = "base_map_test.bin";

            // 路径的前缀名称
            FLAGS_pre_dir = pre_path_base + "/modules/planning/testdata/sunnyvale_loop_test";

            // 要读入的文件  // routing_msg, prediction_msg, localization_msg, chassis_msg 的文件名称
            std::string seq_num = "1";
            FLAGS_test_routing_response_file = seq_num + "_routing.pb.txt";
            FLAGS_test_prediction_file = seq_num + "_prediction.pb.txt";
            FLAGS_test_localization_file = seq_num + "_localization.pb.txt";
            FLAGS_test_chassis_file = seq_num + "_chassis.pb.txt";
            FLAGS_test_traffic_light_file = " ";
        }

        bool FeedTestData()
        {
            // 从文件中读入chassis
            Chassis chassis;
            if (FLAGS_test_chassis_file.empty())
            {
                AERROR << "Requires FLAGS_test_chassis_file to be set";
                return false;
            }
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_chassis_file, &chassis))
            {
                AERROR << "failed to load file: " << FLAGS_test_chassis_file;
                AERROR << (FLAGS_pre_dir + "/" + FLAGS_test_chassis_file);
                return false;
            }

            // 从文件中读入localization
            if (FLAGS_test_localization_file.empty())
            {
                AERROR << "Requires FLAGS_test_localization_file to be set";
                return false;
            }
            LocalizationEstimate localization;
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_localization_file,
                    &localization))
            {
                AERROR << "failed to load file: " << FLAGS_test_localization_file;
                return false;
            }
            Clock::SetMode(Clock::MOCK);
            Clock::SetNowInSeconds(localization.header().timestamp_sec());

            // 从文件中读入prediction
            if (FLAGS_test_prediction_file.empty())
            {
                AERROR << "Requires FLAGS_test_prediction_file to be set";
                return false;
            }
            PredictionObstacles prediction;
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_prediction_file,
                    &prediction))
            {
                AERROR << "failed to load file: " << FLAGS_test_prediction_file;
                return false;
            }

            // 从文件中读入routing_response
            if (FLAGS_test_routing_response_file.empty())
            {
                AERROR << "Requires FLAGS_test_routing_response_file";
                return false;
            }
            RoutingResponse routing_response;
            if (!apollo::cyber::common::GetProtoFromFile(
                    FLAGS_pre_dir + "/" + FLAGS_test_routing_response_file,
                    &routing_response))
            {
                AERROR << "failed to load file: " << FLAGS_test_routing_response_file;
                return false;
            }

            // // 从文件中读入traffic_light_detection  // optional
            // if (FLAGS_test_traffic_light_file.empty())
            // {
            //     AERROR << "Requires FLAGS_test_traffic_light_file";
            //     return false;
            // }
            // TrafficLightDetection traffic_light_detection;
            // if (!apollo::cyber::common::GetProtoFromFile(
            //         FLAGS_pre_dir + "/" + FLAGS_test_traffic_light_file,
            //         &traffic_light_detection))
            // {
            //     AERROR << "failed to load file: " << FLAGS_test_traffic_light_file;
            //     return false;
            // }

            // 组帧为local_view_
            local_view_.prediction_obstacles =
                std::make_shared<PredictionObstacles>(prediction);
            local_view_.chassis = std::make_shared<Chassis>(chassis);
            local_view_.localization_estimate =
                std::make_shared<LocalizationEstimate>(localization);
            local_view_.routing =
                std::make_shared<routing::RoutingResponse>(routing_response);
            // local_view_.traffic_light =
            //     std::make_shared<TrafficLightDetection>(traffic_light_detection);

            AINFO << "Successfully feed proto files.";
            return true;
        }

        void SetUp()
        {
            Set_file_names();

            if (FLAGS_use_navigation_mode)
            {
                // TODO(all)
                // planning_ = std::unique_ptr<PlanningBase>(new NaviPlanning());
            }
            else
            {
                planning_ = std::unique_ptr<PlanningBase>(new OnLanePlanning());
            }

            // 从文件里面读入其他模块的数据
            CHECK(FeedTestData()) << "Failed to feed test data";
            // planning模块的配置文件
            CHECK(cyber::common::GetProtoFromFile(FLAGS_planning_config_file, &config_))
                << "failed to load planning config file " << FLAGS_planning_config_file;
            // 初始化PlanningBase指针
            CHECK(planning_->Init(config_).ok()) << "Failed to init planning module";

            // 如果存在上一帧planning规划出的路径点
            if (!FLAGS_test_previous_planning_file.empty())
            {
                const auto prev_planning_file =
                    FLAGS_pre_dir + "/" + FLAGS_test_previous_planning_file;
                ADCTrajectory prev_planning;
                CHECK(cyber::common::GetProtoFromFile(prev_planning_file, &prev_planning));
                planning_->last_publishable_trajectory_.reset(
                    new PublishableTrajectory(prev_planning));
            }

            // 交通规则的设置
            for (auto &config : *(planning_->traffic_rule_configs_.mutable_config()))
            {
                auto iter = rule_enabled_.find(config.rule_id());
                if (iter != rule_enabled_.end())
                {
                    config.set_enabled(iter->second);
                }
            }
        }

        void UpdateData()
        {
            CHECK(FeedTestData()) << "Failed to feed test data";

            if (!FLAGS_test_previous_planning_file.empty())
            {
                const auto prev_planning_file =
                    FLAGS_pre_dir + "/" + FLAGS_test_previous_planning_file;
                ADCTrajectory prev_planning;
                CHECK(cyber::common::GetProtoFromFile(prev_planning_file, &prev_planning));
                planning_->last_publishable_trajectory_.reset(
                    new PublishableTrajectory(prev_planning));
            }
            for (auto &config : *planning_->traffic_rule_configs_.mutable_config())
            {
                auto iter = rule_enabled_.find(config.rule_id());
                if (iter != rule_enabled_.end())
                {
                    config.set_enabled(iter->second);
                }
            }
        }

        void TrimPlanning(ADCTrajectory *origin,
                          bool no_trajectory_point)
        {
            origin->clear_latency_stats();
            origin->clear_debug();
            // origin->mutable_header()->clear_radar_timestamp();
            // origin->mutable_header()->clear_lidar_timestamp();
            // origin->mutable_header()->clear_timestamp_sec();
            // origin->mutable_header()->clear_camera_timestamp();
            // origin->mutable_header()->clear_sequence_num();

            if (no_trajectory_point)
            {
                origin->clear_total_path_length();
                origin->clear_total_path_time();
                origin->clear_trajectory_point();
            }
        }

        bool IsValidTrajectory(const ADCTrajectory &trajectory)
        {
            for (int i = 0; i < trajectory.trajectory_point_size(); ++i)
            {
                const auto &point = trajectory.trajectory_point(i);

                const double kMaxAccelThreshold =
                    FLAGS_longitudinal_acceleration_upper_bound;
                const double kMinAccelThreshold =
                    FLAGS_longitudinal_acceleration_lower_bound;
                if (point.a() > kMaxAccelThreshold || point.a() < kMinAccelThreshold)
                {
                    AERROR << "Invalid trajectory point because accel out of range: "
                           << point.DebugString();
                    return false;
                }

                if (!point.has_path_point())
                {
                    AERROR << "Invalid trajectory point because NO path_point in "
                              "trajectory_point: "
                           << point.DebugString();
                    return false;
                }

                if (i > 0)
                {
                    const double kPathSEpsilon = 1e-3;
                    const auto &last_point = trajectory.trajectory_point(i - 1);
                    if (point.path_point().s() + kPathSEpsilon <
                        last_point.path_point().s())
                    {
                        AERROR << "Invalid trajectory point because s value error. last point: "
                               << last_point.DebugString()
                               << ", curr point: " << point.DebugString();
                        return false;
                    }
                }
            }
            return true;
        }

        bool RunPlanning(const std::string &test_case_name,
                         int case_num, bool no_trajectory_point)
        {
            const std::string golden_result_file = apollo::common::util::StrCat(
                "result_", test_case_name, "_", case_num, ".pb.txt");

            std::string full_golden_path = FLAGS_pre_dir + "/" + golden_result_file;

            ADCTrajectory adc_trajectory_pb;
            planning_->RunOnce(local_view_, &adc_trajectory_pb);
            printf("planning_->RunOnce   \n");

            if (!IsValidTrajectory(adc_trajectory_pb))
            {
                AERROR << "Fail to pass trajectory check.";
                return false;
            }

            adc_trajectory_ = adc_trajectory_pb;
            TrimPlanning(&adc_trajectory_, no_trajectory_point);
            // 规划出的路径点
            ADEBUG << adc_trajectory_.ShortDebugString();

            if (FLAGS_write_into_file)
                if (1)
                {
                    AINFO << "The golden file is regenerated:" << full_golden_path;
                    cyber::common::SetProtoToASCIIFile(adc_trajectory_, full_golden_path);
                }
                else
                {
                    ADCTrajectory golden_result;
                    bool load_success =
                        cyber::common::GetProtoFromASCIIFile(full_golden_path, &golden_result);
                    TrimPlanning(&golden_result, no_trajectory_point);
                    if (!load_success ||
                        !common::util::IsProtoEqual(golden_result, adc_trajectory_))
                    {
                        char tmp_fname[100] = "/tmp/XXXXXX";
                        int fd = mkstemp(tmp_fname);
                        if (fd < 0)
                        {
                            AERROR << "Failed to create temporary file: " << tmp_fname;
                            return false;
                        }
                        if (!cyber::common::SetProtoToASCIIFile(adc_trajectory_, fd))
                        {
                            AERROR << "Failed to write to file: " << tmp_fname;
                        }
                        AERROR << "found error\ndiff -y " << tmp_fname << " " << full_golden_path;
                        AERROR << "to override error\nmv " << tmp_fname << " "
                               << full_golden_path;
                        AERROR << "to visualize\n/usr/bin/python "
                                  "modules/tools/plot_trace/plot_planning_result.py "
                               << tmp_fname << " " << full_golden_path;
                        return false;
                    }
                }
            return true;
        }

        TrafficRuleConfig *GetTrafficRuleConfig(
            const TrafficRuleConfig::RuleId &rule_id)
        {
            for (auto &config : *planning_->traffic_rule_configs_.mutable_config())
            {
                if (config.rule_id() == rule_id)
                {
                    return &config;
                }
            }
            return nullptr;
        }

    } // namespace planning
} // namespace apollo

using std::chrono::high_resolution_clock;
using std::chrono::milliseconds;
int main()
{
    apollo::planning::SetUp();

    for (int i = 0; i < 1000; i++)
    {
        // 开始时间
        high_resolution_clock::time_point beginTime = high_resolution_clock::now();

        // 从文件里面读入其他模块的数据
        apollo::planning::FeedTestData();
        // plannng的入口函数
        apollo::planning::RunPlanning("planning_demo", 1, false);

        // 结束时间
        high_resolution_clock::time_point endTime = high_resolution_clock::now();
        milliseconds timeInterval = std::chrono::duration_cast<milliseconds>(endTime - beginTime);
        std::cout << "planning used  " << timeInterval.count() << " ms\n";

        sleep(1);
    }
    return 0;
}